#include "stdafx.h"
#include ".\matrix.h"
#include "math.h"

mdsMatrix<3, double> matrixGlobals::GetRotAboutVector(const mdsVector<3, double> & vecAxis, double lfTheta)
{
	// consider re-using this code in a quaternion library later
	mdsMatrix<3, double> mResult;
	if(vecAxis.Dot(vecAxis) < GenericMath::eps)
	{
		return mResult;
	}


	double lfQuatRealComp = cos(0.5*lfTheta);
	mdsVector<3, double> quatIJKcomp(vecAxis);
	quatIJKcomp.Normalize();

	quatIJKcomp *= sin(0.5*lfTheta);

	//Matrix = [ 1 - 2yy - 2zz    2xy - 2wz      2xz + 2wy
	//            2xy + 2wz    1 - 2xx - 2zz    2yz - 2wx
	//            2xz - 2wy      2yz + 2wx    1 - 2xx - 2yy ]

	double wx2 = 2.0*lfQuatRealComp*quatIJKcomp.x();
	double wy2 = 2.0*lfQuatRealComp*quatIJKcomp.y();
	double wz2 = 2.0*lfQuatRealComp*quatIJKcomp.z();

	double xx2 = 2.0*quatIJKcomp.x() * quatIJKcomp.x();
	double xy2 = 2.0*quatIJKcomp.x() * quatIJKcomp.y(); // redundant
	double xz2 = 2.0*quatIJKcomp.x() * quatIJKcomp.z(); // redundant

	// double yx2 = 2.0*quatIJKcomp.y() * quatIJKcomp.x();
	double yy2 = 2.0*quatIJKcomp.y() * quatIJKcomp.y();
	double yz2 = 2.0*quatIJKcomp.y() * quatIJKcomp.z(); // redundant

	// double zx2 = 2.0*quatIJKcomp.z() * quatIJKcomp.x();
	// double zy2 = 2.0*quatIJKcomp.z() * quatIJKcomp.y();
	double zz2 = 2.0*quatIJKcomp.z() * quatIJKcomp.z();

	//Matrix = [ 1 - 2yy - 2zz    2xy - 2wz      2xz + 2wy
	//            2xy + 2wz    1 - 2xx - 2zz    2yz - 2wx
	//            2xz - 2wy      2yz + 2wx    1 - 2xx - 2yy ]
	// taken from "GameDev.net"
	// since the article didn't specify a matrix convention I am going to assume
	// that vectors are post-multiplied in their convention.
	// I will simply swap conventions by changing indices in my copy of this conversion

	mResult[0][0] = 1.0-yy2-zz2; mResult[1][0] = xy2 - wz2;   mResult[2][0] = xz2 + wy2;
	mResult[0][1] = xy2 + wz2;   mResult[1][1] = 1.0-xx2-zz2; mResult[2][1] = yz2 - wx2;
	mResult[0][2] = xz2 - wy2;   mResult[1][2] = yz2 + wx2;   mResult[2][2] = 1-xx2-yy2;

	return mResult;
	
}

mdsMatrix<3, double> matrixGlobals::GetMatRotComponent(const mdsMatrix<4, double> &matIn)
{
	mdsMatrix<3, double> mResult;
	for(int i = 0; i < 3; ++i)
	{
		for(int j = 0; j < 3; ++j)
		{
			mResult[i][j] = matIn[i][j];
		}
	}
	return mResult;
}

mdsMatrix<4, double> matrixGlobals::RotMatToProjectiveXform(const mdsMatrix<3, double> & matIn)
{
	mdsMatrix<4, double> mResult;
	for(int i = 0; i < 3; ++i)
	{
		for(int j = 0; j < 3; ++j)
		{
			mResult[i][j] = matIn[i][j];
		}
	}
	return mResult;
}

mdsVector<4, double> matrixGlobals::Vec3ProjectUp(const mdsVector<3, double> & vecIn)
{
	mdsVector<4, double> vecResult;
	vecResult[0] = vecIn[0]; vecResult[1] = vecIn[1]; vecResult[2] = vecIn[2];
	vecResult[3] = 1.0;
	return vecResult;
}

mdsVector<3, double> matrixGlobals::Vec4ProjectDown(const mdsVector<4, double> &vecIn)
{
	mdsVector<3, double> vecResult;
	vecResult *= (1.0/vecIn[3]); // potential divide-by-zero
	return vecResult;
}



template<class ARG_TYPE> double matrixGlobals::GetDet(const ARG_TYPE * glMat, int dim)
{
	
	if(dim == 1)
	{
		return (double)(*glMat);
	}
	if(dim == 2)
	{
		return (double)(glMat[0]*glMat[3] - glMat[2]*glMat[1]);
	}

	// 0 1 2
	// 3 4 5
	// 6 7 8
	if(dim == 3)
	{
		return (double)(glMat[0])*(glMat[4]*glMat[8] - glMat[5]*glMat[7]) 
			- (double)(glMat[1])*(glMat[3]*glMat[8] - glMat[5]*glMat[6])
			+ (double)(glMat[2])*(glMat[3]*glMat[7] - glMat[4]*glMat[6]);
	}

	if(dim < 1)
	{
		return 1.0;
	}

	ARG_TYPE * pTmpArr = new ARG_TYPE[(dim-1)*(dim-1)];

	double lfRunningSum = 0.0;
	for(int i = 0; i < dim; ++i)
	{
		RemoveRowColumn(glMat, pTmpArr, dim, 0, i);
		double lfCurrCofactor = GetDet(pTmpArr, dim-1);
		lfRunningSum += (1.0 - 2.0*(i%2))*glMat[i]*lfCurrCofactor;
	}
	delete [] pTmpArr;

	return lfRunningSum;
}

template<class ARG_TYPE> void matrixGlobals::GetCofactorMat(const ARG_TYPE * glMatIn, ARG_TYPE * glMatOut, int dim)
{
	if(dim < 1)
	{
		return;
	}
	if(dim == 1)
	{
		*glMatOut = *glMatIn;
		return;
	}
	ARG_TYPE * pMatTmp = new ARG_TYPE[(dim-1)*(dim-1)];

	for(int i = 0; i < dim; ++i)
	{
		double signumI = (double)(1-2*(i%2));
		for(int j = 0; j < dim; ++j)
		{
			double signumJ = (double)(1-2*(j%2));
			RemoveRowColumn(glMatIn, pMatTmp, dim, i, j);
			glMatOut[(dim*i+j)] = (ARG_TYPE)(signumI*signumJ*GetDet(pMatTmp, dim-1));
		}
	}

	delete [] pMatTmp;

}

template<class ARG_TYPE> void matrixGlobals::RemoveRowColumn(const ARG_TYPE * glMatIn, ARG_TYPE * glMatOut, int dim, int nRow, int nCol)
{
	for(int i = 0; i < dim-1; ++i)
	{
		for(int j = 0; j < dim-1; ++j)
		{
			int iRow = i+nRow;
			int jCol = j+nCol;
			int idxOut = (dim-1)*(iRow%(dim-1))+(jCol%(dim-1));
			int idxIn = dim*((iRow+1)%dim)+((jCol+1)%dim);

			glMatOut[idxOut] =
				glMatIn[idxIn];
		}
	}
}

template<class ARG_TYPE> bool matrixGlobals::InvertMat(const ARG_TYPE * glMatIn, ARG_TYPE * glMatOut, int dim)
{
	double lfDet = GetDet(glMatIn, dim);
	if(fabs(lfDet) < GenericMath::eps)
	{
		return false;
	}
	lfDet = 1.0/lfDet;
	GetCofactorMat(glMatIn, glMatOut, dim);
	for(int i = 0; i < dim; ++i)
	{
		glMatOut[(dim+1)*i] = (ARG_TYPE)(lfDet*glMatOut[(dim+1)*i]);
		for(int j = 1+i; j < dim; ++j)
		{
			double lfTmp = glMatOut[dim*j+i];
			glMatOut[dim*j+i] = (ARG_TYPE)(lfDet*glMatOut[dim*i+j]);
			glMatOut[dim*i+j] = (ARG_TYPE)(lfDet*lfTmp);
		}
	}
	return true;
}

template<class ARG_TYPE> void matrixGlobals::m4ToGlMat(const mdsMatrix<4, double> & matIn, ARG_TYPE * glMatOut)
{
	
	for(int i = 0; i < 4; ++i)
	{
		for(int j = 0; j < 4; ++j)
		{
			glMatOut[4*i+j] = matIn[i][j]; // no need to transpose since I already work with premultiply conventions
		}
	}
}

template<class ARG_TYPE> void matrixGlobals::glMatToM4(const ARG_TYPE * glMatIn, mdsMatrix<4, double> & matOut)
{
	for(int i = 0; i < 4; ++i)
	{
		for(int j = 0; j < 4; ++j)
		{
			matOut[i][j] = glMatIn[4*i+j];
		}
	}
}



mdsMatrix<4, double> matrixGlobals::InvertMdsMat(const mdsMatrix<4, double> & matIn)
{
	
	double tmpArrIn[16];
	double tmpArrOut[16];
	m4ToGlMat(matIn, tmpArrIn);
	if(InvertMat(tmpArrIn, tmpArrOut, 4))
	{
		mdsMatrix<4, double> matResultSuccess(false);
		glMatToM4(tmpArrOut, matResultSuccess);
		return matResultSuccess;
	}
	mdsMatrix<4, double> matResultFailure;
	return matResultFailure;
}


double GenericMath::eps = 0.000000001;
double GenericMath::pi = 3.1415926535897931;
double GenericMath::halfPi = 1.5707963267948966;
double GenericMath::twoPi = 6.2831853071795862;
double GenericMath::piOver2 = 1.5707963267948966;
double GenericMath::piOver4 = 0.78539816339744828;
double GenericMath::piOver3 = 1.0471975511965976;
double GenericMath::piOver6 = 0.52359877559829882;

double GenericMath::sqrt3 = 1.7320508075688772;
double GenericMath::sqrt2 = 1.4142135623730951;

double GenericMath::oneOverSqrt2 = 1.4142135623730951;
double GenericMath::oneOverSqrt3 = 0.57735026918962584;

int OctUtils::QuadTraverseOrder[8] = {0, 1,2,4, 3,5,6, 7};
int OctUtils::OctCornerGreyCode[8] = {0, 1, 3, 2, 6, 4, 5, 7}; 


